/*
 * Servo.c
 *
 *  Created on: Nov 29, 2012
 *      Author: gmkado
 */
#include "hw_types.h"
#include "pwm.h"
#include "hw_memmap.h"
#include "sysctl.h"
#include "gpio.h"
#include "Servo.h"
#include <math.h>

#define DELAY_PER_DEGREE 50000
#define MAX_WIDTH 2.3
#define MIN_WIDTH .4
#define MAX_ANGLE 180.
#define SERVO_HOME 90.
#define MIN_ANGLE 0.

static angle_t currAngle;

void initServo(void)
{
   unsigned long ulPeriod;

    //
    // Set the clocking to run directly from the crystal.
    //

    SysCtlPWMClockSet(SYSCTL_PWMDIV_4);

    //
    // Enable the peripherals used by this example.
    //
    SysCtlPeripheralEnable(SYSCTL_PERIPH_PWM);
    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOH);

    //
    // Set GPIO H0 as PWM pin.
    //
    GPIOPinTypePWM(GPIO_PORTH_BASE, GPIO_PIN_0);

    //
    // Compute the PWM period based on the system clock.
    //
    ulPeriod = SysCtlClockGet()/4/ 50;

    //
    // Set the PWM period to 50 Hz.
    //
    PWMGenConfigure(PWM_BASE, PWM_GEN_1, PWM_GEN_MODE_UP_DOWN | PWM_GEN_MODE_NO_SYNC);
    PWMGenPeriodSet(PWM_BASE, PWM_GEN_1, ulPeriod);

	// send servo to center location
	moveServo(SERVO_HOME);

    //
    // Enable the PWM2 output signals.
    //
    PWMOutputState(PWM_BASE, PWM_OUT_2_BIT, true);

    //
    // Enable the PWM generator.
    //
    PWMGenEnable(PWM_BASE, PWM_GEN_1);



}

void moveServo(angle_t angle)
{
	float m = (MAX_WIDTH-MIN_WIDTH)/(MAX_ANGLE-MIN_ANGLE);
	float b = (MIN_WIDTH*MAX_ANGLE - MAX_WIDTH*MIN_ANGLE)/(MAX_ANGLE-MIN_ANGLE);
    float pulseWidth = m*angle+b;
    unsigned long pulseTicks = pulseWidth/1000*SysCtlClockGet()/4;
	PWMPulseWidthSet(PWM_BASE, PWM_OUT_2, pulseTicks);

	//TODO: fix this to work with timer interrupts
	angle_t delta = abs(currAngle - angle);
	delayServo(DELAY_PER_DEGREE*delta);
	currAngle = angle;
}

/* Move servo counterclockwise STEPANGLE degrees*/
void moveServoCCW(angle_t stepAngle)
{
	angle_t newAngle = currAngle + stepAngle;
	moveServo(newAngle);
}

/* Move servo counterclockwise STEPANGLE degrees*/
void moveServoCW(angle_t stepAngle)
{
	angle_t newAngle = currAngle - stepAngle;
	moveServo(newAngle);
}


angle_t getServoAngle(void)
{
	return currAngle;
}

void delayServo(unsigned int numTicks)
{
	int i;
	for(i = 0;  i<numTicks; i++);
}

